
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Battery.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Battery.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static const struct
{
    float volt_per_cell;    //每s电池电压
    float soc_pct;
}soc_table[] = 
{
    { 4.173f, 100.0f },
    { 4.112f, 96.15f },
    { 4.085f, 92.31f },
    { 4.071f, 88.46f },
    { 4.039f, 84.62f },
    { 3.987f, 80.77f },
    { 3.943f, 76.92f },
    { 3.908f, 73.08f },
    { 3.887f, 69.23f },
    { 3.854f, 65.38f },
    { 3.833f, 61.54f },
    { 3.801f, 57.69f },
    { 3.783f, 53.85f },
    { 3.742f, 50.0f },
    { 3.715f, 46.15f },
    { 3.679f, 42.31f },
    { 3.636f, 38.46f},
    { 3.588f, 34.62f },
    { 3.543f, 30.77f },
    { 3.503f, 26.92f },
    { 3.462f, 23.08f },
    { 3.379f, 19.23f },
    { 3.296f, 15.38f },
    { 3.218f, 11.54f },
    { 3.165f, 7.69f },
    { 3.091f, 3.85f },
    { 2.977f, 2.0f },
    { 2.8f,   1.5f },
    { 2.7f,   1.3f },
    { 2.5f,   1.2f },
    { 2.3f,   1.1f },
    { 2.1f,   1.0f },
    { 1.9f,   0.9f },
    { 1.6f,   0.8f },
    { 1.3f,   0.7f },
    { 1.0f,   0.6f },
    { 0.6f,   0.4f },
    { 0.3f,   0.2f },
    { 0.01f,  0.01f},
    { 0.001f, 0.001f}
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static float Battery_GetRestVoltage(Battery* battery, float charge_pct);
static void Battery_SetInitSoc(Battery* battery, float voltage);

//设置电池模型参数
void Battery_Setup(Battery* battery, float _capacity_Ah, float _resistance, float _max_voltage)
{
    battery->_capacity_ah = _capacity_Ah;
    battery->_resistance = _resistance;
    battery->_max_voltage = _max_voltage;
    lpf_set_cutoff1(&(battery->volta_filt), 10.0f);
}

//配置初始状态电压
void Battery_InitVoltage(Battery* battery, float voltage)
{
    //复位滤波器输入
    lpf_reset(&(battery->volta_filt), voltage);
    battery->voltage_set = voltage;
    Battery_SetInitSoc(battery, voltage);
}

//设置输出电流  current(A) t(s)
void Batterty_SetCurrent(Battery* battery, float current, float dt)
{
    //uint64_t now = 0.001;
    //float dt = (now - battery->last_us) * 1.0e-6;
    if (dt > 0.1f)
    {
        //停止更新
        dt = 0;
    }
    //battery->last_us = now;
    //消耗的电量
    float delta_Ah = current * dt / 3600;
    battery->rem_ah -= delta_Ah;
    battery->rem_ah = MAX(0, battery->rem_ah);

    //计算降压
    float delta_v = current * battery->_resistance;
    float voltage;
    if (!math_flt_positive(battery->_capacity_ah))
    {
        voltage = battery->voltage_set;
    }
    else
    {
        voltage = Battery_GetRestVoltage(battery, 100 * battery->rem_ah / battery->_capacity_ah) - delta_v;
    }
    //更新滤波输出
    lpf_apply2(&(battery->volta_filt), voltage, dt);
}

float Batterty_GetVoltage(Battery* battery)
{
    return lpf_get_output(&(battery->volta_filt));
}

//根据剩余电量百分比，计算输出电压
static float Battery_GetRestVoltage(Battery* battery, float charge_pct)
{
    const float max_cell_voltage = soc_table[0].volt_per_cell;

    for (uint8_t cnt = 1; cnt < ARRAY_SIZE(soc_table); cnt++)
    {
        if (charge_pct >= soc_table[cnt].soc_pct)
        {
            //线性插值
            float cell_volt = math_linear_inter(soc_table[cnt].volt_per_cell, soc_table[cnt - 1].volt_per_cell,
                charge_pct, soc_table[cnt].soc_pct, soc_table[cnt - 1].soc_pct);
            return (cell_volt / max_cell_voltage) * battery->_max_voltage;
        }
    }
    //无电量，返回近似零的数  避免出现错误（除零）
    return 0.001f;
}

//根据输出电压 计算剩余电量
static void Battery_SetInitSoc(Battery* battery, float voltage)
{
    const float max_cell_voltage = soc_table[0].volt_per_cell;
    float cell_volt = (voltage / battery->_max_voltage) * max_cell_voltage;

    for (uint8_t cnt = 1; cnt < ARRAY_SIZE(soc_table); cnt++)
    {
        if (cell_volt >= soc_table[cnt].volt_per_cell)
        {
            float soc = math_linear_inter(soc_table[cnt].soc_pct, soc_table[cnt - 1].soc_pct,
                cell_volt, soc_table[cnt].volt_per_cell, soc_table[cnt - 1].volt_per_cell);
            battery->rem_ah = battery->_capacity_ah * soc * 0.01f;
            return;
        }
    }

    battery->rem_ah = 0.0f;
}

/*------------------------------------test------------------------------------*/


